#ifndef NAV2_CUSTOM_CONTROLLER_HPP
#define NAV2_CUSTOM_CONTROLLER_HPP


#include "rclcpp/rclcpp.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_util/robot_utils.hpp"

namespace nav2_custom_controller
{
    class CustomController : public nav2_core::Controller
    {
    private:
    nav2_util::LifecycleNode::SharedPtr node_;
    std::string name_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
    nav_msgs::msg::Path global_path_;
    double max_linear_speed_,max_angular_speed_;
    // 获取路径中距离当前点最近的点
    geometry_msgs::msg::PoseStamped getNearTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);
    // 计算目标点方向和当前朝向的角度差
    double calculateAngleDifference(const geometry_msgs::msg::PoseStamped &current_pose,const geometry_msgs::msg::PoseStamped &target_pose);

    public:
        CustomController() = default;
        ~CustomController() = default;

        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

        void cleanup() override;

        void activate() override;

        void deactivate() override;

        void setPlan(const nav_msgs::msg::Path &path) override;

        geometry_msgs::msg::TwistStamped computeVelocityCommands(
            const geometry_msgs::msg::PoseStamped &pose,
            const geometry_msgs::msg::Twist &velocity,
            nav2_core::GoalChecker *goal_checker) override;

        void setSpeedLimit(const double &speed_limit, const bool &percentage) override;
    };

} // nav2_custom_controller

#endif // NAV2_CUSTOM_CONTROLLER_HPP
